home *** CD-ROM | disk | FTP | other *** search
/ C/C++ Interactive Reference Guide / C-C++ Interactive Reference Guide.iso / c_ref / csource5 / 341_01 / k2ce.c < prev    next >
Text File  |  1991-02-27  |  6KB  |  201 lines
  1. /*
  2. HEADER:       k2ce.c        
  3. TITLE:        Keplerian-to-Cartesian conversion routine         
  4. VERSION:      1.0    
  5. DESCRIPTION:  This routine converts the six classical Keplerian orbital
  6.               elements to Cartesian xyz position and velocity elements.
  7.               Required inputs are the six Keplerian elements plus mu for
  8.               the central body.      
  9. KEYWORDS:     Astrodynamics, orbital mechanics, Keplerian elements,
  10.               Cartesian coordinates    
  11. SYSTEM:       MS-DOS, PC-DOS (Coded with Ver. 3.3, but should be version
  12.               independent)         
  13. FILENAME:     k2ce.c    
  14. UNITS:        I believe that the code is not dependent on any particular
  15.               set of units.  See orbcons.h for the value of earth mu
  16.               I use.
  17. WARNINGS:      This version is for elliptical input only.
  18.               Input elements should have a > 0 and e < 1.
  19.               This routine was coded for educational purposes, following
  20.               the development in the reference.  No attempt has been
  21.               made to provide conversions which are optimally numerically
  22.               stable.            
  23. SEE-ALSO:     c2ke.c    
  24. AUTHORS:      Rodney Long
  25.               19003 Swan Drive
  26.               Gaithersburg, MD 20879     
  27. COMPILERS:    Microsoft C 5.1    
  28. REFERENCE:    Bate, Mueller, White: Fundamentals of Astrodynamics
  29. */
  30.  
  31. #include <stdio.h>
  32. #include <float.h>
  33. #include <math.h>
  34. #include "orbcons.h"
  35.  
  36. double sk();
  37.  
  38. main()
  39. {
  40.   double c[6],k[6],mu;
  41.   char filename[31];
  42.   FILE *fp;
  43.   int rc;
  44.  
  45.   printf("Enter input file name: \n");
  46.   gets(filename);
  47.  
  48.  
  49.   fp = fopen(filename,"r");
  50.   printf("filename = %s\n",filename);
  51.   if ( fp == NULL) {
  52.     perror("Open error \n");
  53.     exit(0);
  54.   }
  55.   fscanf(fp,"%lf %lf %lf",&k[0],&k[1],&k[2]);
  56.   fscanf(fp,"%lf %lf %lf",&k[3],&k[4],&k[5]);
  57.   fscanf(fp,"%lf",&mu); 
  58.   printf("Input elements (a,e,i,lan,w,m): \n");
  59.   printf("%25.16lf %25.16lf %25.16lf\n",k[0],k[1],k[2]); 
  60.   printf("%25.16lf %25.16lf %25.16lf\n",k[3],k[4],k[5]);
  61.   printf("Input mu: %25.16lf\n",mu);
  62.  
  63.  
  64.   k[2] *= DTR;
  65.   k[3] *= DTR;
  66.   k[4] *= DTR;
  67.   k[5] *= DTR;
  68.  
  69.   rc = k2c(k,mu,c);
  70.  
  71.   if (rc == -1 ) {
  72.     printf("Input orbit is not elliptic. \n");
  73.   } else {
  74.     printf("Output pos/vel: \n");
  75.     printf("%25.16lf %25.16lf %25.16lf\n",c[0],c[1],c[2]); 
  76.     printf("%25.16lf %25.16lf %25.16lf\n",c[3],c[4],c[5]);
  77.   }
  78. }
  79.  
  80. //  This routine converts classical Keplerian elements to
  81. //    Cartesian position and velocity.
  82.  
  83. //  Input : k[6] -- a, e, i, lan, w, m    ; 
  84. //          mu   -- mu of central body
  85. //  Output: c[6] -- x, y, z, xdot, ydot, zdot  
  86.  
  87.   k2c (double *k, double mu, double *c)
  88. {   
  89.         int max = 10;
  90.       double tol = .5e-16;
  91.       double r11, r21, r31;
  92.       double r12, r22, r32;
  93.       double cee, see;
  94.       double ci, cw, clan; 
  95.       double si, sw, slan; 
  96.       
  97.  
  98.       double a, e, ee, e0, i, lan, m, w;
  99.       double xp,yp,xpd,ypd, r;
  100.  
  101.       // Get input vector elements into local variables
  102.       //   with logical names.
  103.       a   = k[0];       // Semi-major axis.
  104.       e   = k[1];       // Eccentricity.
  105.       i   = k[2];       // Inclination.  (rad)
  106.       lan = k[3];       // Longitude of ascending node. (rad)
  107.       w   = k[4];       // Argument of perigee. (rad)  
  108.         m   = k[5]*RTD;   // Mean anomaly. (deg)
  109.  
  110.       // If orbit is elliptical, proceed.  If not
  111.       //  print warning and halt.
  112.         if ( a > 0  & e < 1  ) { 
  113.         // Orbit is elliptical.
  114.       
  115.         e0 = m;                   // Make Kepler starting iterate
  116.                                   //  = input mean anomaly.
  117.         // Call sk() to solve Kepler.
  118.         ee = sk(m,e0,e,max,tol);
  119.                                              
  120.         ee  = ee * DTR;           // Eccen anomaly is output in
  121.                                   //   degrees by sk(); convert it
  122.                                   //   to radians.          
  123.         cee = cos(ee);            // Cos and sin of
  124.         see = sin(ee);            //  eccen anomaly output
  125.                                   //  by Kepler solution.
  126.  
  127.         r    = a * (1 - e * cee);        // Compute orbit radius at
  128.                                          //   current position. 
  129.       
  130.         xp   = a * (cee - e);            // Compute perifocal
  131.         yp   = a * (sqrt(1-e*e) * see);  //   (x,y) of position.
  132.  
  133.         xpd  = (-sqrt(mu*a)*see)/r;         // Compute perifocal
  134.         ypd  = sqrt(mu/(a * (1-e*e) ) ) *  // (x,y) of velocity. 
  135.                 (e + (e - cee)/(e * cee - 1) );
  136.  
  137.         } else { 
  138.         printf("Input orbit is not elliptic. \n");
  139.         return(-1);
  140.  
  141.       }
  142.  
  143.       // Now compute remaining pos/vel coordinates.
  144.  
  145.       ci   =  cos(i);     // Cos, sin
  146.       si   =  sin(i);     //   of inclination.
  147.       clan =  cos(lan);   // Cos, sin
  148.       slan =  sin(lan);   //   of longitude of ascending node.
  149.       cw   =  cos(w);     // Cos, sin
  150.       sw   =  sin(w);     //   of arg of perigee.
  151.  
  152.       // Compute matrix to convert from perifocal
  153.       //   to geocentric-equatorial coordinates.
  154.       r11   =  cw * clan - sw * slan * ci;
  155.       r21   =  cw * slan + sw * clan * ci;
  156.       r31   =  sw * si;
  157.       r12   = -sw * clan - cw * slan * ci;
  158.       r22   = -sw * slan + cw * clan * ci;
  159.       r32   =  cw * si; 
  160.  
  161.  
  162.       // Now apply the matrix to compute
  163.       //   the output position and velocity.
  164.  
  165.       c[0] = r11 * xp  + r12 * yp;
  166.       c[1] = r21 * xp  + r22 * yp;
  167.       c[2] = r31 * xp  + r32 * yp;
  168.       c[3] = r11 * xpd + r12 * ypd;
  169.       c[4] = r21 * xpd + r22 * ypd;
  170.       c[5] = r31 * xpd + r32 * ypd;
  171.  
  172.       return(0);
  173.  
  174.  }
  175.  
  176.   
  177. double sk(double m,double e0,double e,int mi,double t)
  178. {
  179.   /* Solve Kepler's equation by the Newton method. */
  180.   double en,ep;
  181.   int i;
  182.  
  183.   e0 = e0 * DTR;
  184.   m  = m  * DTR;
  185.  
  186.   i  = 0;
  187.   en = e0;
  188.   do {
  189.     i++;
  190.     ep = en;
  191.  
  192.     // The next line of code is the Newton iteration:
  193.     //   x(n+1) = x(n) - (1/(dy/dx)) * y(n). 
  194.  
  195.     en = en - (en - e*sin(en) - m)/(1 - e*cos(en))   ;
  196.     en = fmod(en,TWOPI);
  197.     if ( fabs(en-ep) < t) break;
  198.   } while (i <= mi );
  199.   return(en*RTD);
  200. }
  201.